#include "dynamicmodel.h"

DynamicModel::DynamicModel() :
    mPitch(0), mYaw(0), mRoll(0),
    mGravity(0, 0, -9.81),
    mMass(1.0)
{
}

void DynamicModel::integrate(float dt)
{
    integrateStep(dt);
}

void DynamicModel::reset()
{
    mPosition = QVector3D();
    mVelocity = QVector3D();
    mAcceleration = QVector3D();
    mRotation = QQuaternion();
    mAngularVelocity = QVector3D();
    mAngularAcceleration = QVector3D();
    mForce = QVector3D();
    mMomentum = QVector3D();
}

void DynamicModel::integrateStep(float h)
{
    QVector3D &g = mGravity;
    float m = mMass;
    float J = m*0.2*0.2; // from balda
    float J_1 = 1 / J;
//    QMatrix3x3 &J = mInertialTensor;
//    QMatrix3x3 J_1 = 4toto tipa: J^(-1);
    QVector3D &x = mPosition;
    QVector3D &v = mVelocity;
    QVector3D &a = mAcceleration;
    QQuaternion &q = mRotation;
    QVector3D &w = mAngularVelocity;
    QVector3D &e = mAngularAcceleration;
    QVector3D &Fe = mForce;
    QVector3D &Me = mMomentum;

    QVector3D F = Fe + m*g;
    QVector3D M = Me;
    a = F / m;
    e = M * J_1;

    v += a * h;
    w += e * h;

    x += v * h;
    if (!w.isNull())
        q = QQuaternion::fromAxisAndAngle(w.normalized(), w.length()*180/M_PI*h) * q;

    float qx = q.x();
    float qy = q.y();
    float qz = q.z();
    float qw = q.scalar();

    mEx = QVector3D(1 - 2*qy*qy - 2*qz*qz, 2*qx*qy + 2*qz*qw, 2*qx*qz - 2*qy*qw);
    mEy = QVector3D(2*qx*qy - 2*qz*qw, 1 - 2*qx*qx - 2*qz*qz, 2*qy*qz + 2*qx*qw);
    mEz = QVector3D(2*qx*qz + 2*qy*qw, 2*qy*qz - 2*qx*qw, 1 - 2*qx*qx - 2*qy*qy);

    mYaw = atan2f(2*qx*qy + 2*qz*qw, 1 - 2*qy*qy - 2*qz*qz);
    mPitch = +100500;//asinf(2*qy*qw + 2*qx*qz);
    mRoll = +100500;//atan2f(2*qx*qw - 2*qy*qz , 1 - 2*qx*qx - 2*qz*qz);

//    if (qx*qy + qz*qw == 0.5)
//    {
//        mYaw = 2 * atan2(qx, qw);
//        mRoll = 0;
//    }
//    else if (qx*qy + qz*qw == -0.5)
//    {
//        mYaw = -2 * atan2(qx, qw);
//        mRoll = 0;
//    }
}
